#include "VI530x_User_Handle.h"
#include <rtthread.h>
#include <rtdevice.h>
#include <rtdbg.h>
#include "zino.h"
#include "sensordata.h"
#include "fc_error.h"
/* Start user code for adding. */

/* End user code.  */
static struct rt_i2c_bus_device *vi_i2c_bus = RT_NULL; /* I2C总线设备句柄 */

/***********************************************************
 * 此文档需要客户移植完成
 * 1、IIC读写函数
 * 2、实现延时函数：void VI530x_Delay_Ms(uint16_t nMs)
 * 3、如果使用硬件中断,则在触发下降沿中断中调用：void VI530x_GPIO_Interrupt_Handle(void)
 * 4、XSHUT高低电位的控制：void VI530x_XSHUT_Enable(uint8_t state)
 * Start user code for adding--End user code 空间请补充
 * ********************************************************/

/* Start user code for adding. */
// Xshut引脚配置
// #define VI530x_XSHUT_GPIO_Port	GPIOA
// #define VI530x_XSHUT_Pin	GPIO_PIN_0

// IIC读/写函数
/**dev_addr：设备地址，默认0xD8
 *addr：寄存器地址
 *value/pValue：读/写数据缓存区地址
 *tlen：长度
 *return：[uint8_t]0-操作成功（I2C读写无异常）;other-异常（I2C读写有异常）
 **/
// uint8_t IIC_Write_X_Bytes(uint8_t dev_addr, uint8_t addr, uint8_t *pValue, uint16_t tlen);
// uint8_t IIC_Read_X_Bytes(uint8_t dev_addr, uint8_t addr, uint8_t *value, uint16_t tlen);

/* End user code.  */

/**
 * @brief 	VI530X I2C 1Byte 读
 * @param 	[uint8_t] addr：读地址
 * @param 	[uint8_t] *value：读到的值
 * @return 	[uint8_t]	ret:0-操作成功（I2C读写无异常）;other-异常（I2C读写有异常）
 */
VI530x_Status VI530x_IIC_Read_One_Byte(uint8_t reg, uint8_t *value)
{

	struct rt_i2c_msg vi_msgs[2];

	vi_i2c_bus = (struct rt_i2c_bus_device *)rt_device_find("i2c2");
	vi_msgs[0].addr = VI530x_IIC_DEV_ADDR >> 1;
	vi_msgs[0].buf = &reg;
	vi_msgs[0].len = 1;
	vi_msgs[0].flags = RT_I2C_WR;

	vi_msgs[1].addr = VI530x_IIC_DEV_ADDR >> 1;
	vi_msgs[1].buf = value;
	vi_msgs[1].len = 1;
	vi_msgs[1].flags = RT_I2C_RD;
	// struct rt_i2c_bit_ops *ops = (struct rt_i2c_bit_ops *)vi_i2c_bus->priv;

	if (rt_i2c_transfer(vi_i2c_bus, vi_msgs, 2) != 2)
	{
		return VI530x_IIC_ERROR;
	}
	return RT_EOK;
}

/**
 * @brief 	VI530X I2C x Byte 读
 * @param 	[uint8_t] addr：读地址
 * @param 	[uint8_t] *value：读到的值
 * @param 	[uint16_t] tlen：读取的长度
 * @return 	[uint8_t]	ret:0-操作成功（I2C读写无异常）;other-异常（I2C读写有异常）
 */
VI530x_Status VI530x_IIC_Read_X_Bytes(uint8_t reg, uint8_t *value, uint16_t tlen)
{
	struct rt_i2c_msg vi_msgs[2];
	vi_msgs[0].addr = VI530x_IIC_DEV_ADDR >> 1;
	vi_msgs[0].buf = &reg;
	vi_msgs[0].len = 1;
	vi_msgs[0].flags = RT_I2C_WR;

	vi_msgs[1].addr = VI530x_IIC_DEV_ADDR >> 1;
	vi_msgs[1].buf = value;
	vi_msgs[1].len = tlen;
	vi_msgs[1].flags = RT_I2C_RD;
	// struct rt_i2c_bit_ops *ops = (struct rt_i2c_bit_ops *)vi_i2c_bus->priv;
	// ops->delay_us = 0;
	if (rt_i2c_transfer(vi_i2c_bus, vi_msgs, 2) != 2)
	{
		return VI530x_IIC_ERROR;
	}
	return RT_EOK;
}

/**
 * @brief 	VI530X I2C 1 Byte 写
 * @param 	[uint8_t] addr：写的地址
 * @param 	[uint8_t] value：写入的值
 * @return 	[uint8_t]	ret:0-操作成功（I2C读写无异常）;other-异常（I2C读写有异常）
 */
VI530x_Status VI530x_IIC_Write_One_Byte(uint8_t reg, uint8_t value)
{
	struct rt_i2c_msg vi_msgs;
	uint8_t buf[2] = {reg, value};
	vi_msgs.addr = VI530x_IIC_DEV_ADDR >> 1;
	vi_msgs.buf = buf;
	vi_msgs.len = 2;
	vi_msgs.flags = RT_I2C_WR;

	// struct rt_i2c_bit_ops *ops = (struct rt_i2c_bit_ops *)vi_i2c_bus->priv;
	// ops->delay_us = 0;
	if (rt_i2c_transfer(vi_i2c_bus, &vi_msgs, 1) != 1)
	{
		return VI530x_IIC_ERROR;
	}
	return RT_EOK;
}

/**
 * @brief 	VI530X I2C 1 Byte 写
 * @param 	[uint8_t] addr：写的地址
 * @param 	[uint8_t] *pValue：写入的值
 * @param 	[uint16_t] tlen：写入的长度
 * @return 	[uint8_t]	ret:0-操作成功（I2C读写无异常）;other-异常（I2C读写有异常）
 */
VI530x_Status VI530x_IIC_Write_X_Bytes(uint8_t reg, uint8_t *pValue, uint16_t tlen)
{
	struct rt_i2c_msg vi_msgs[2];

	vi_msgs[0].addr = VI530x_IIC_DEV_ADDR >> 1;
	vi_msgs[0].buf = &reg;
	vi_msgs[0].len = 1;
	vi_msgs[0].flags = RT_I2C_WR;

	vi_msgs[1].addr = VI530x_IIC_DEV_ADDR >> 1;
	vi_msgs[1].buf = pValue;
	vi_msgs[1].len = tlen;
	vi_msgs[1].flags = RT_I2C_WR | RT_I2C_NO_START;
	// struct rt_i2c_bit_ops *ops = (struct rt_i2c_bit_ops *)vi_i2c_bus->priv;
	// ops->delay_us = 0;
	if (rt_i2c_transfer(vi_i2c_bus, vi_msgs, 2) != 2)
	{
		return VI530x_IIC_ERROR;
	}
	return RT_EOK;
}

/**
 * @brief 	VI530X 延时 单位：ms
 * @param 	[none]
 * @return 	[none]
 */
void VI530x_Delay_Ms(uint16_t nMs)
{
	/* Start user code for adding. */
	// 必须满足大于等于1*nMs ms
	/* End user code.  */
	rt_thread_mdelay(nMs);
}

// 添加于输入中断函数内调用
/**
 * @brief 	VI530X 延时 硬件中断处理
 * @param 	[none]
 * @return 	[none]
 */
void VI530x_GPIO_Interrupt_Handle(void)
{
	if (VI530x_Cali_Data.VI530x_Interrupt_Mode_Status)
	{
		VI530x_GPIO_Interrupt_status = 1;
	}
}

/**
 * @brief 	VI530X XSHUS引脚控制
 * @param 	[uint8_t] state：0-拉低，1-拉高
 * @return 	[none]
 */
void VI530x_XSHUT_Enable(uint8_t state)
{
	if (state)
	{
		// Xshut输出高电平
		/* Start user code for adding. */

		/* End user code.  */
	}
	else
	{
		// Xshut输出低电平
		/* Start user code for adding. */

		/* End user code.  */
	}
}

VI530x_MEASURE_TypeDef result;
// 移植流程
void VI530x_main(void *p)
{
	VI530x_Status ret = VI530x_OK;
	// VI530x_MEASURE_TypeDef result;

	// 1、IIC 初始化
	// 支持1M
	/* Start user code for adding. */

	/* End user code.  */

	// 2、GPIO 初始化
	// （a）驱动配置xshut管脚----上拉输出
	// （b）如果使用硬件GPIO中断，则驱动配置GPIO管脚----下降沿输入中断
	/* Start user code for adding. */

	/* End user code.  */

	// 3、选择中断方式：0x00----寄存器0x03查询，其他值----GPIO硬件中断
	// VI530x_Cali_Data.VI530x_Interrupt_Mode_Status = 0x00;		//软件中断，GPIO引脚直接上拉
	//  VI530x_Cali_Data.VI530x_Interrupt_Mode_Status = 0x88;		//硬件中断，GPIO引脚接主控

	// 4、VI530x寄存器初始化
	ret |= VI530x_Chip_Init();
	rt_kprintf("VI530x_Chip_Init ret = %d\n", ret);
	// 5、VI530x固件写入
	ret |= VI530x_Download_Firmware((uint8_t *)VI5300_M31_firmware_buff, FirmwareSize());
	rt_kprintf("VI530x_Download_Firmware ret = %d\n", ret);
	// 6、标定值配置
#if 1
	/************************* 标定后正常测距流程 *****************************/
	// 从主控端NVM读取标定值
	/* Start user code for adding. */

	/* End user code.  */
	/** 没有标定前用于测试的缺省标定值  **/
	//	VI530x_Cali_Data.VI530x_Calibration_Offset = 0;
	//	VI530x_Cali_Data.VI530x_Calibration_CG_Pos = 0;
	//  VI530x_Cali_Data.VI530x_Calibration_CG_Maxratio = 10;
	//	ret |= VI530x_Get_Sys_Reftof(&VI530x_Cali_Data.VI530x_Calibration_Reftof);

	// 配置标定值
	//	VI530x_Cali_Data.VI530x_Calibration_CG_Maxratio += 10;	//增加Xtalk处理上限，正常不需要启用
	ret |= VI530x_Set_Californiation_Data(VI530x_Cali_Data.VI530x_Calibration_Offset);
	rt_kprintf("VI530x_Set_Californiation_Data ret = %d\n", ret);
#else
	/************************* 生产标定流程 *****************************/
	// ref_tof标定
	ret |= VI530x_Reftof_Calibration();
	if (ret == 0)
	{
		rt_kprintf("VI530x_Calibration_Reftof = %4d\r\n", VI530x_Cali_Data.VI530x_Calibration_Reftof);
	}
	else
	{
		rt_kprintf("RefTof Calibration Fail!\r\n");
	}
	// Xtalk标定，环境要求60cm内无目标物
	ret |= VI530x_Xtalk_Calibration();
	if (ret == 0)
	{
		rt_kprintf("VI530x_Calibration_CG_Pos = %d\r\n", VI530x_Cali_Data.VI530x_Calibration_CG_Pos);
		rt_kprintf("VI530x_Calibration_CG_Maxratio = %d\r\n", VI530x_Cali_Data.VI530x_Calibration_CG_Maxratio);
		/***！Xtalk卡控，建议在标定工装上卡控，便于根据结构调整  ***/
		if (VI530x_Cali_Data.VI530x_Calibration_CG_Maxratio > 15)
		{
			// 检测结构Xtalk大小，盖板为主要影响，可以根据实际情况微调
			rt_kprintf("Xtalk is too large, Fail!\r\n");
		}
	}
	else
	{
		rt_kprintf("Xtalk Calibration Fail!\r\n");
	}

	// Offset标定
	// 在固定距离做offset标定,如10cm，则把参数改成100（mm）
	ret |= VI530x_Offset_Calibration(VI530x_OFFSET_DISTANCE);
	if (ret == 0)
	{
		rt_kprintf("VI530x_Calibration_Offset = %f\r\n", VI530x_Cali_Data.VI530x_Calibration_Offset);
	}
	else
	{
		rt_kprintf("Offset Calibration Fail!\r\n");
	}
	// 保存标定值在主控NVM
	/*注意：模组只需做一次标定即可，之后将以下标定的值保存
	VI530x_Cali_Data.VI530x_Calibration_CG_Pos			//与结构相关，一般值在区域-10~+10
	VI530x_Cali_Data.VI530x_Calibration_CG_Maxratio //与结构相关，一般值在区域 0~+10
	VI530x_Cali_Data.VI530x_Calibration_Reftof			//一般值在区域 300~720
	VI530x_Cali_Data.VI530x_Calibration_Offset 			//与电特性和结构相关，一般值在区域 -50~+50
	********************************************/
	/* Start user code for adding. */

	/* End user code.  */

#endif

	// 7、其它配置
	// 开启温度校准:0x00-关，0x01-开，请做了RefTof标定后再开户温度校准
	ret |= VI530x_Set_Sys_Temperature_Enable(0x01);
	ret |= VI530x_Set_Integralcounts_Frame(30, 131072); // 帧率，积分次数

	// 8、开启测距
	ret |= VI530x_Start_Continue_Ranging_Cmd(); // 连续模式
	// ret = VI530x_Start_Single_Ranging_Cmd();	//单次模式,功耗低

	if (ret)
	{
		rt_kprintf("VI530x Configer Error!\r\n");
		FC_ERROR_SET(FC_LASER_HARDFAULT);
	}
	else
	{
		rt_kprintf("VI530x Configer Ok!\r\n");
		FC_ERROR_RESET(FC_LASER_HARDFAULT);
	}

	while (1)
	{
		/* Start user code for adding. */

		/* End user code.  */
		ret = VI530x_Get_Measure_Data(&result);
		if (!ret)
		{
			// ！！建议confidece大于70，ToF值为可信
			sensorData.tof.range = (uint16_t)result.correction_tof;
			sensorData.tof.quality = result.confidence;
			// rt_kprintf("tof = %4d, confidece = %3d\r\n", sensorData.tof.range, sensorData.tof.quality);
			/* 参数说明：
			result.correction_tof：距离值，毫米为单位；
			result.confidence：表示当前 TOF 值的可信度，建议大于70可信，具体可以根据应用调整；
			result.peak：表征接收到光信号强度；
			******************/
			// 开启单次测距，单次测距调用只会输出1次距离值
			// ret = VI530x_Start_Single_Ranging_Cmd();
		}
		rt_thread_mdelay(10);
	}
}

int vi53_task_entry(void)
{
	rt_thread_t vi53_data_thread = rt_thread_create("vi53_data_thread", VI530x_main, RT_NULL, 1024, 10, 10);
	if (vi53_data_thread != RT_NULL)
	{
		rt_thread_startup(vi53_data_thread);
	}
	else
	{
		LOG_E("sensor_data_thread create failed.");
	}
	return 0;
}

ZINO_APP_EXPORT(vi53_task_entry);